#pragma once
namespace graph_basic
{
   namespace math
   {
      inline lat_lon_t grad_to_rad(lat_lon_t fi)
      {
          lat_lon_t pi = 3.1415926535f;
          return pi*fi/180;
      }
      inline lat_lon_t rad_to_grad(lat_lon_t rad)
      {
          lat_lon_t pi = 3.1415926535f;
          return rad*180/pi;
      }
      inline lat_lon_t euclid_distance(lat_lon_t lat1,lat_lon_t lng1,lat_lon_t lat2,lat_lon_t lng2)
      {
          //printf("lat1:%lf lat2:%lf lat1-lat2:%lf eukl %lf \n", lat1, lat2, lat1 - lat2, sqrt( pow(lat1-lat2,2) + pow(lng1-lng2,2) ) );
          return sqrt( pow(lat1-lat2,2) + pow(lng1-lng2,2) );
      }
      inline lat_lon_t manhattan_distance(lat_lon_t lat1,lat_lon_t lng1,lat_lon_t lat2,lat_lon_t lng2)
      {
          return  abs(lat1 - lat2) + abs(lng1 - lng2);
      }
      inline lat_lon_t sphere_distance(lat_lon_t lat1,lat_lon_t lng1,lat_lon_t lat2,lat_lon_t lng2)
      {
          lat1 = grad_to_rad(lat1);
          lat2 = grad_to_rad(lat2);
          lng1 = grad_to_rad(lng1);
          lng2 = grad_to_rad(lng2);
          lat_lon_t d_lambda = lng2 - lng1;
          lat_lon_t d_fi = lat2 - lat1;
          ////high precision
          //lat_lon_t d_sigma = atan( 
          //    sqrt( pow(cos(lat2)*sin(d_lambda),2) + pow( cos(lat1)*sin(lat2) - sin(lat1)*cos(lat2)*cos(d_lambda),2 ) )
          //    / ( sin(lat1)*sin(lat2) + cos(lat1)*cos(lat2)*cos(d_lambda) )
          //    );
          lat_lon_t d_sigma = 2*asin( sqrt( pow(sin(d_fi/2),2) + cos(lat1)*cos(lat2)*pow(sin(d_lambda/2),2)) );

          return d_sigma * 6378.1370f;
          
          //low precision
          //return 6378.1370f * acos( cos(lat1) * cos(lat2) + sin(lat1) * sin(lat2) * cos(lng1 - lng2)); //usuall
      }
      // r = 6,378.1370
      // pi 3.1415926535 
      inline lat_lon_t distance(lat_lon_t lat1,lat_lon_t lng1,lat_lon_t lat2,lat_lon_t lng2)
      {
          return  sphere_distance(lat1,lng1,lat2,lng2);
      }
      inline point_t edge_middle_point(edge_t const &  e, graph_t& g)
      {
          vertex_t& source_v = source(e, g);
          vertex_t& target_v = target(e, g);
          
          return point_t( static_cast<float>(0.5*(source_v.get_point().lat_ + target_v.get_point().lat_)),
                                      static_cast<float>(0.5*(source_v.get_point().lon_ + target_v.get_point().lon_))
              );
      }
      inline lat_lon_t my_astar_heuristic(vertex_t& cur_vertex, vertex_t& dist_vertex)
      {          
          return distance(cur_vertex.get_point().lat_, cur_vertex.get_point().lon_,
                          dist_vertex.get_point().lat_, dist_vertex.get_point().lon_);
      }
      inline lat_lon_t my_astar_heuristic(vertex_t& cur_vertex, lat_lon_t lat2, lat_lon_t lng2)
      {      
          return distance(cur_vertex.get_point().lat_, cur_vertex.get_point().lon_, lat2, lng2);
      }

      //XY are global positions, not screen positions
      inline point_t lat_lon_to_xy(lat_lon_t lat, lat_lon_t lon, lat_lon_t lon0)
      {
          lat_lon_t C=1000;
          lat_lon_t pi = 3.1415926535f;
          point_t result;
          result.lat_ = C*grad_to_rad(lon - lon0);
          result.lon_ = C*log(tan(grad_to_rad(lat)/2 + pi/4)) - 1300;
          //printf("%f: %f | %f: %f \n",lat, result.second, lon, result.first);
          return result;
      }
      inline point_t xy_to_lat_lon(lat_lon_t x, lat_lon_t y, lat_lon_t lon0)
      {
          lat_lon_t C = 1000;
          lat_lon_t pi = 3.1415926535f;
          point_t result;
          lat_lon_t e = 2.71828f;
          result.lon_ = (rad_to_grad(x/C) + lon0); //lon
          result.lat_ = rad_to_grad( 2* atan(exp((y + 1300)/C)) - pi/2 ); //lat
          return result;
      }
   }
}